#!/usr/bin/env python


import math
import time

import rospy
import actionlib
from move_base_msgs.msg import *


class SimServer(object):
    def __init__(self, *args, **kwargs):
        super(SimServer, self).__init__(*args, **kwargs)
        self.position = {'x': 0, 'y': 0}

        self.server = actionlib.SimpleActionServer("move_base", MoveBaseAction, self.execute, False)
        self.server.start()
        return

    def execute(self, goal):
        rospy.loginfo("%s accept." % str(self.server.current_goal))
        rospy.loginfo("%s" % str(goal))

        position, orientation = goal.target_pose.pose.position, goal.target_pose.pose.orientation
        position = {'x': position.x, 'y': position.y}
        
        count = int(math.sqrt((position['x']-self.position['x'])**2 + (position['y']-self.position['y'])**2)) + 1
        
        self.position.update(position)

        for index in range(0, count):
            if self.server.is_preempt_requested():
                rospy.loginfo("%s preempt occurred." % str(self.server.current_goal))
                self.server.set_preempted()
                return
            time.sleep(1)
        self.server.set_succeeded()
        return


if __name__ == "__main__":
    rospy.init_node("sim_server")
    server = SimServer()
    rospy.spin()


